#include <Arduino.h>
#include <hardware/adc.h>
#include <hardware/dma.h>
#include <hardware/timer.h>
#include <hardware/irq.h>
#include "arduinoFFT.h"


//u8g2
#include <U8g2lib.h>
#define DISPLAY_BINS 64
#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif
// #include"drawCiallo.h"



// 采样配置
#define ADC_PIN 26      // GPIO26 (ADC0)
#define SAMPLE_RATE 16384
#define BUF_SIZE SAMPLE_RATE   // 缓冲区大小


// 双缓冲
uint16_t buffer0[BUF_SIZE];
uint16_t buffer1[BUF_SIZE];
volatile uint16_t* active_buffer = buffer0;
volatile bool ready_buffer = true; 
volatile bool fftTimeDomain_waitForCalculate = false;
uint16_t* fftTimeDomain;

// DMA相关变量
int dma_chan;
dma_channel_config dma_config;

//u8g2
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);
    float fftDisplayData[DISPLAY_BINS];
    float majorPeak = 0;
    float semiPeak=0;
    uint32_t fftComputeTime = 0;
    uint32_t fftEmptyTime=0;
    uint32_t plotTime=0;

// 定时器中断处理函数
void __not_in_flash_func(adc_timer_handler)() {
    // 手动触发ADC采样
    hw_set_bits(&adc_hw->cs, ADC_CS_START_ONCE_BITS);
    
    // 清除定时器中断
    hw_clear_bits(&timer_hw->intr, 1u << 0);
}

void dma_handler() {
    ready_buffer = !ready_buffer;
    active_buffer = (active_buffer == buffer0) ? buffer1 : buffer0; // 切换活动缓冲区
    dma_channel_configure(dma_chan, &dma_config, (void*)active_buffer, &adc_hw->fifo, BUF_SIZE, true);
    
    if(!fftTimeDomain_waitForCalculate) {
        // rp2040.memcpyDMA(fftTimeDomain, ready_buffer ? buffer1 : buffer0, 2 * BUF_SIZE);
        fftTimeDomain=ready_buffer ? buffer1 : buffer0;
        fftTimeDomain_waitForCalculate = true;
    }
    
    dma_channel_acknowledge_irq0(dma_chan);
}

// 绘制FFT频谱图


#define SYS_CLK_FREQ 200*1000000    //MHZ
#define ADC_CLK_DIV ((clock_get_hz(clk_sys)/SAMPLE_RATE)/4)-96-24

void setup() {

    // //oled setup'
    // u8g2.begin();
    // u8g2.clearBuffer();
    // u8g2.setFont(u8g2_font_6x10_tf);
    // u8g2.drawStr(0, 10, "Initializing...");
    // u8g2.sendBuffer();
    // Serial.begin(115200);


    // 1. 初始化ADC
    adc_init();
    adc_gpio_init(ADC_PIN);
    adc_select_input(1); // ADC0
    adc_fifo_setup(true, true, 1, false, false);
    adc_set_clkdiv(ADC_CLK_DIV);//min:96

    // 2. 配置定时器中断（替代PIO）
    hw_set_bits(&timer_hw->inte, 1u << 0); // 启用定时器中断
    irq_set_exclusive_handler(TIMER_IRQ_0, adc_timer_handler);
    irq_set_enabled(TIMER_IRQ_0, true);
    
    // 设置定时器频率为20KHz
    timer_hw->alarm[0] = timer_hw->timerawl + (clock_get_hz(clk_sys) / SAMPLE_RATE);

    // 3. 配置DMA
    dma_chan = dma_claim_unused_channel(true);
    dma_config = dma_channel_get_default_config(dma_chan);
    
    channel_config_set_transfer_data_size(&dma_config, DMA_SIZE_16);
    channel_config_set_read_increment(&dma_config, false);
    channel_config_set_write_increment(&dma_config, true);
    channel_config_set_dreq(&dma_config, DREQ_ADC);
    
    // 中断配置
    dma_channel_set_irq0_enabled(dma_chan, true);
    irq_set_exclusive_handler(DMA_IRQ_0, dma_handler);
    irq_set_enabled(DMA_IRQ_0, true);
    irq_set_priority(DMA_IRQ_0, PICO_HIGHEST_IRQ_PRIORITY);

    // 4. 启动第一个DMA传输
    dma_channel_configure(
        dma_chan,
        &dma_config,
        buffer0,
        &adc_hw->fifo,
        BUF_SIZE,
        true
    );
    
    // 5. 启动ADC
    adc_run(true);
    
    // Serial.println("ADC采样系统已启动,20KHz采样率(定时器中断驱动)");
    delay(3000);
}
bool prev;

void loop() {
    // // 测试时用于校准
    // const uint32_t expect_sample_period=BUF_SIZE*1e6/SAMPLE_RATE;
    // float proportion=(fftEmptyTime+fftComputeTime+plotTime)/expect_sample_period;
    // if(abs(proportion-1)<=0.001 && (!prev && fftTimeDomain_waitForCalculate)){
        
    //     // uint32_t offset=ADC_CLK_DIV/(1-proportion);
    //     ADC_CLK_DIV/=proportion;
    //     Serial.print("ADC_CLK_DIV adjusted to");Serial.println(ADC_CLK_DIV);

    //     adc_set_clkdiv(ADC_CLK_DIV);
    // }
    // prev=fftTimeDomain_waitForCalculate;

}

// FFT处理部分（核心1）
float vReal[BUF_SIZE];
float vImag[BUF_SIZE];
const uint16_t samplingFrequency = SAMPLE_RATE;
// #define FFT_SPEED_OVER_PRECISION
ArduinoFFT<float> FFT = ArduinoFFT<float>(vReal, vImag, BUF_SIZE, samplingFrequency);




void drawCiallo() {
    const unsigned char cialloBitmap [] PROGMEM = {
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7c, 0x03, 0x36, 
        0x00, 0x00, 0x18, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x00, 0x00, 0x08, 0x00, 0x46, 0x00, 0x36, 0x00, 
        0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x80, 0x01, 0x00, 0x0c, 0x00, 0x02, 0x00, 0x36, 0x00, 0x00, 
        0x0c, 0x18, 0x00, 0x00, 0x00, 0x03, 0x01, 0x00, 0x0c, 0x00, 0x03, 0x7b, 0x36, 0x0f, 0x00, 0x0c, 
        0x0c, 0x00, 0x0c, 0x83, 0x01, 0x01, 0xc0, 0xff, 0x00, 0x03, 0xc3, 0xb6, 0x19, 0x6f, 0x04, 0x06, 
        0x0c, 0x04, 0x62, 0x00, 0x03, 0x00, 0x7f, 0x00, 0x03, 0xf3, 0xb6, 0x18, 0x39, 0x04, 0x03, 0x0c, 
        0x64, 0x76, 0x00, 0xc3, 0x07, 0x1e, 0x00, 0x02, 0xcb, 0xb6, 0x18, 0x00, 0x84, 0x01, 0x00, 0x64, 
        0xc6, 0x01, 0x63, 0x0c, 0x1e, 0x00, 0x46, 0xcb, 0xb6, 0x19, 0x00, 0xcc, 0x00, 0x00, 0x6c, 0x02, 
        0x03, 0x31, 0x08, 0x37, 0x00, 0x7c, 0xfb, 0x36, 0x0f, 0x00, 0xec, 0x1f, 0x00, 0xfc, 0x03, 0x00, 
        0x11, 0x18, 0x21, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x80, 0x01, 
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x00, 0x00, 
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
    };
    u8g2.setFont(u8g2_font_unifont_t_chinese2);
    u8g2.clearBuffer();
    // u8g2.setFont(u8g2_font_unifont_t_symbols);
    // u8g2.drawUTF8(10, 20, "Ciallo~");
    // u8g2.drawUTF8(10, 40, "(∠·w<)⌒★");
    u8g2.drawXBMP((128-114)/2, 24, 114, 16, cialloBitmap);
    u8g2.sendBuffer();
}
void drawHotaru(){
    const unsigned char hotaru [] PROGMEM = {
        // 'b_811ea2fa93f536dc33ad0921bcd84d11, 64x64px
        0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x37, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0xe7, 
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0xe7, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 
        0x30, 0x00, 0x0f, 0x00, 0x00, 0x00, 0x00, 0xc7, 0xf8, 0xc1, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x8f, 
        0xdc, 0xc3, 0x19, 0x00, 0x00, 0xf0, 0xff, 0x9f, 0x0e, 0xff, 0xf1, 0x07, 0x00, 0xfe, 0xff, 0x9f, 
        0x1c, 0xfe, 0xf0, 0x0f, 0xc0, 0x5f, 0x57, 0xff, 0x78, 0x07, 0x00, 0x0c, 0xf8, 0x9b, 0xeb, 0xfa, 
        0xf0, 0x07, 0x00, 0x0c, 0xfc, 0xd8, 0xef, 0xef, 0xc0, 0x7f, 0xbe, 0x07, 0x3f, 0xc2, 0xef, 0xdd, 
        0x3c, 0x3c, 0x1e, 0x83, 0xaf, 0x7f, 0xf7, 0x81, 0xfe, 0x1f, 0x1f, 0xe7, 0xcf, 0xfb, 0xff, 0xfb, 
        0xc6, 0x0f, 0x0f, 0xfe, 0xfd, 0xc0, 0x8b, 0xfe, 0x06, 0x07, 0x00, 0xfc, 0x3a, 0x00, 0x57, 0xfd, 
        0x9e, 0x07, 0xfc, 0x7c, 0x1c, 0x00, 0xbe, 0xfe, 0xf8, 0xff, 0xff, 0x1f, 0x07, 0x80, 0x7f, 0xfd, 
        0xe0, 0xfc, 0xff, 0xff, 0x03, 0xe0, 0xf3, 0x7e, 0xe0, 0xcd, 0xb9, 0xd7, 0x01, 0xf0, 0xf7, 0xbd, 
        0xe0, 0xcf, 0xd9, 0x6b, 0x00, 0xb8, 0xbe, 0x7f, 0x70, 0xce, 0xf9, 0x71, 0x00, 0x3e, 0x31, 0x9f, 
        0x30, 0xce, 0xf9, 0x38, 0x00, 0x7f, 0x28, 0xee, 0x38, 0xcf, 0x79, 0x3d, 0x80, 0x6f, 0x14, 0x9d, 
        0x1c, 0xc7, 0xb9, 0x3e, 0xc0, 0xc1, 0xaa, 0x7a, 0x8c, 0xe7, 0xf9, 0x37, 0xe0, 0xf8, 0xff, 0xff, 
        0xce, 0xe7, 0x9d, 0xf3, 0xf1, 0xf9, 0xff, 0xff, 0xc6, 0xe3, 0xed, 0xe3, 0xff, 0x7f, 0x83, 0xef, 
        0xee, 0xb3, 0xff, 0x0f, 0x8e, 0xf7, 0xc3, 0xc9, 0x7c, 0xbf, 0xdf, 0x0f, 0xc6, 0xe6, 0x7b, 0x98, 
        0x30, 0x0e, 0x57, 0x08, 0xaf, 0xee, 0x3f, 0xb0, 0x00, 0x00, 0x6e, 0x88, 0x57, 0xff, 0x1e, 0x60, 
        0x00, 0x00, 0x76, 0xd8, 0xa9, 0x9a, 0x0f, 0x78, 0x00, 0x00, 0x7e, 0xf8, 0x7c, 0xbd, 0x0f, 0x7e, 
        0x80, 0x1f, 0x77, 0xf8, 0xf8, 0xb2, 0x1f, 0xcf, 0xfc, 0xff, 0x6b, 0xf0, 0x54, 0xb5, 0xb7, 0xc7, 
        0xfe, 0xf0, 0xd7, 0xf0, 0xb8, 0xe6, 0xe7, 0xc3, 0x06, 0x00, 0xcb, 0xf8, 0x39, 0xf1, 0xe7, 0xf9, 
        0x06, 0x00, 0xd7, 0xdc, 0xf3, 0xe2, 0xc3, 0xd9, 0xfe, 0xf1, 0x9b, 0xfc, 0xf6, 0xf1, 0x03, 0xef, 
        0xfc, 0xff, 0xd7, 0xd7, 0xee, 0xff, 0x07, 0xbe, 0xf8, 0xff, 0x8d, 0xe7, 0xbc, 0xff, 0x0f, 0x00, 
        0x1c, 0x80, 0x8d, 0xc3, 0xfe, 0xff, 0x0f, 0x00, 0x06, 0x80, 0x15, 0xc3, 0xff, 0x7f, 0x00, 0x00, 
        0xc6, 0xff, 0x39, 0xc7, 0x67, 0x7f, 0x00, 0x00, 0xc6, 0xff, 0x25, 0xf7, 0xc3, 0x00, 0x00, 0x00, 
        0xfe, 0x1f, 0xeb, 0xee, 0x81, 0x01, 0x00, 0x00, 0xfc, 0x00, 0x57, 0xff, 0x00, 0x03, 0x00, 0x00, 
        0x30, 0x00, 0xeb, 0x7e, 0x80, 0x03, 0x00, 0x00, 0x30, 0x1e, 0xff, 0x3d, 0xc0, 0x07, 0xe0, 0x00, 
        0x30, 0x1e, 0xfb, 0xde, 0xe0, 0x06, 0xe0, 0x80, 0x30, 0x1e, 0x77, 0xfd, 0x78, 0x06, 0x20, 0xc0, 
        0x30, 0x00, 0xea, 0xbe, 0x9f, 0x07, 0x00, 0xf0, 0x70, 0x00, 0xf6, 0x77, 0xcf, 0x07, 0x00, 0xf8, 
        0xe0, 0x1f, 0xef, 0xe2, 0xce, 0x06, 0x00, 0x3c, 0xf8, 0x1f, 0xdf, 0xe7, 0xf9, 0x03, 0x00, 0x1c, 
        0xfe, 0x1f, 0xef, 0x92, 0xf3, 0x01, 0x00, 0x9e, 0x06, 0x00, 0x6c, 0x39, 0x0f, 0x00, 0x00, 0x8e, 
        0x06, 0x00, 0x4c, 0x19, 0x7e, 0x00, 0x00, 0xce, 0x04, 0xfe, 0x8f, 0x2a, 0xf4, 0x03, 0x00, 0xce, 
        0xfc, 0xff, 0x1f, 0x21, 0x88, 0x01, 0xf0, 0x7f, 0x38, 0x00, 0xb8, 0x32, 0xe0, 0x00, 0xf8, 0x7f, 
        0x00, 0x00, 0xf0, 0x75, 0x78, 0x00, 0xdc, 0x7d, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
    };
    u8g2.clearBuffer();
    // u8g2.setFont(u8g2_font_unifont_t_symbols);
    // u8g2.drawUTF8(10, 20, "Ciallo~");
    // u8g2.drawUTF8(10, 40, "(∠·w<)⌒★");
    u8g2.drawXBMP(31, 0, 64, 64, hotaru);
    u8g2.sendBuffer();
}

void setup1() {
    // delay(1000); // 等待串口初始化完成
    //oled setup'
    u8g2.begin();
    u8g2.clearBuffer();

    // u8g2.drawStr(0, 10, "Initializing...");
    // drawCiallo();
    drawHotaru();

    u8g2.setFont(u8g2_font_6x10_tf);
    Serial.begin(115200);
    // delay(500);
    
}
uint32_t timecache;

class KalmanFilter {
private:
    float gain;          // 卡尔曼增益
    float output;        // 当前状态估计
    float output_predict;// 预测状态
    float process_noise; // 过程噪声协方差 (Q)
    float predict_conv;  // 预测协方差 (P_k|k-1)
    float state_conv;    // 状态协方差 (P_k|k)
    float observe_conv;  // 观测噪声协方差 (R)
    bool is_initialized; // 是否已初始化
public:
    // 构造函数初始化
    KalmanFilter(float init_observe_conv, float init_state_conv, float q_noise = 0.1f) 
        : observe_conv(init_observe_conv), 
          state_conv(init_state_conv), 
          process_noise(q_noise),
          is_initialized(false) {
        output = 0;          // 初始状态估计（临时值，第一次观测后更新）
        output_predict = 0;   // 初始预测状态
        predict_conv = 0;     // 初始预测协方差
    }
    float input(float observe) {
        if (!is_initialized) {
            // 第一次观测，直接设为初始状态
            output = observe;
            output_predict = observe;
            is_initialized = true;
            return output;
        }
        // 1. 预测步骤
        output_predict = output;  // x_k|k-1 = A * x_k-1|k-1 (A=1)
        predict_conv = state_conv + process_noise;  // P_k|k-1 = A * P_k-1|k-1 * A' + Q
        // 2. 更新步骤
        gain = predict_conv / (predict_conv + observe_conv);  // K = P_k|k-1 * H' / (H * P_k|k-1 * H' + R) (H=1)
        output = output_predict + gain * (observe - output_predict);  // x_k|k = x_k|k-1 + K * (z_k - H * x_k|k-1)
        state_conv = (1 - gain) * predict_conv;  // P_k|k = (I - K * H) * P_k|k-1
        return output;
    }
};

KalmanFilter filter(1.0,2048,0.1);  //    KalmanFilter(float init_observe_conv, float init_state_conv, float q_noise = 0.1f) 

void drawFFT(bool twoPeaks) {
    u8g2.clearBuffer();
    
    // 绘制标题
    u8g2.setFont(u8g2_font_6x10_tf);
    // u8g2.drawStr(0, 10, "FFT Spectrum");
    int max=0;
    for(int i=0;i<DISPLAY_BINS;i++){
        max=fftDisplayData[i]>max?fftDisplayData[i]:max;
    }
    // 绘制频谱柱状图
    for(uint8_t i = 0; i < DISPLAY_BINS; i++) {
        uint8_t barHeight = constrain(map(fftDisplayData[i], 0, max, 0, 50), 0, 50)+1;
        u8g2.drawVLine(i*2 + 10, 63 - barHeight, barHeight);
    }
    
    // 显示主频信息
    int row=1;
    char Str[20];
    if(majorPeak>semiPeak){
        float temp=majorPeak;
        majorPeak=semiPeak;
        semiPeak=temp;
    }
    if(twoPeaks){
        snprintf(Str, sizeof(Str), "Peak1: %.1f Hz",majorPeak);
        // snprintf(Str, sizeof(Str), "Peak: %.1f/%.1f Hz",filter.input(majorPeak),majorPeak);
        u8g2.drawStr(0, 2+8*row, Str);
        row++;
        snprintf(Str, sizeof(Str), "Peak2: %.1f Hz",semiPeak);
        // snprintf(Str, sizeof(Str), "Peak: %.1f/%.1f Hz",filter.input(majorPeak),majorPeak);
        u8g2.drawStr(0, 2+8*row, Str);
        row++;
    }
    else{
        snprintf(Str, sizeof(Str), "Peak: %.1f Hz",majorPeak);
        // snprintf(Str, sizeof(Str), "Peak: %.1f/%.1f Hz",filter.input(majorPeak),majorPeak);
        u8g2.drawStr(0, 2+8*row, Str);
        row++;
    }
    // 显示处理时间
    snprintf(Str, sizeof(Str), "%d ms/%d dot", fftComputeTime/1000,BUF_SIZE);
    u8g2.drawStr(0, 2+8*row, Str);
    row++;

    snprintf(Str, sizeof(Str), "%d ms per sample", (fftEmptyTime+fftComputeTime+plotTime)/1000,BUF_SIZE);
    u8g2.drawStr(0, 2+8*row, Str);
    row++;

    
    // 显示采样信息
    // char sampleStr[20];
    // snprintf(sampleStr, sizeof(sampleStr), "%dHz %dpts", SAMPLE_RATE, BUF_SIZE);
    // u8g2.drawStr(70, 40, sampleStr);
    u8g2.sendBuffer();
}


void loop1() {
    if(fftTimeDomain_waitForCalculate) {
        fftEmptyTime=micros()-timecache;
        timecache=micros();
        
        for(int i = 0; i < BUF_SIZE; i++) {
            vReal[i] = (float)fftTimeDomain[i];
            vImag[i] = 0.0;
        }
        
        FFT.windowing(vReal,BUF_SIZE,FFT_WIN_TYP_HANN,FFT_FORWARD);
        FFT.dcRemoval();
        FFT.compute(FFTDirection::Forward);
        
        // majorPeak = filter.input(FFT.majorPeak());
        majorPeak=FFT.majorPeakParabola();
        
        int peakIndex = (majorPeak / (SAMPLE_RATE/(float)BUF_SIZE))+0.5;
        float majorpeak_value=vReal[peakIndex];
        for (int i = peakIndex - 2; i <= peakIndex + 2; i++) {
            if (i >= 0 && i < BUF_SIZE / 2) {
                vReal[i] = 0;
            }
        }
        semiPeak=FFT.majorPeakParabola();
        peakIndex=(semiPeak / (SAMPLE_RATE/(float)BUF_SIZE))+0.5;
        float semiPeak_value=vReal[peakIndex];
        for(int i = 0; i < DISPLAY_BINS; i++) {
            uint16_t startBin = i * (BUF_SIZE/2 / DISPLAY_BINS);
            uint16_t endBin = (i+1) * (BUF_SIZE/2 / DISPLAY_BINS);
            fftDisplayData[i] = 0;
            for(int j = startBin; j < endBin; j++) {
                fftDisplayData[i] += vReal[j+5];
            }
            fftDisplayData[i] /= (endBin - startBin);
        }
        fftComputeTime=micros()-timecache;
        timecache=micros();
        drawFFT(true);
        
        
        plotTime=micros()-timecache;
        timecache=micros();
        fftTimeDomain_waitForCalculate = false;
        // Serial.print("Size of FFT:");Serial.println(sizeof(FFT)); 
    }
}